/*
卡尔曼滤波算法

参数说明:
Q_angle：相对于加速度计的噪音协方差。   Q增大，动态响应变快，收敛稳定性变坏
Q_bias：相对于陀螺仪的噪音协方差。	Q增大，动态响应变快，收敛稳定性变坏
R_measure：测量噪声协方差（有的版本是R_angle说是ACC的协方差）。 R增大，动态响应变慢，收敛稳定性变好 


例：  
  //对Roll角和Pitch角进行卡尔曼滤波,这里的realVals[4]和[5]是陀螺仪xy轴角速度(即roll和pitch角速度)。 
  //fRoll和fPitch是计算到的即时roll/pitch值（角度)
  float fNewRoll = kalmanRoll.getAngle(fRoll, realVals[4], dt);		//dt单位是秒
  float fNewPitch = kalmanPitch.getAngle(fPitch, realVals[5], dt);
  
  //跟据滤波值计算角度速
  float fRollRate = (fNewRoll - fLastRoll) / dt;
  float fPitchRate = (fNewPitch - fLastPitch) / dt;
 
  //更新Roll角和Pitch角
  fLastRoll = fNewRoll;
  fLastPitch = fNewPitch;
*/



#ifndef _Kalman_h_
#define _Kalman_h_

class Kalman {
public:
    Kalman();
    
	//angle-单位为度, rate-单位为度/秒,dt-单位为秒
    float getAngle(float newAngle, float newRate, float dt);
	
	

    void setAngle(float angle); // Used to set angle, this should be set as the starting angle
    float getRate(); // Return the unbiased rate

    /* These are used to tune the Kalman filter */
    void setQangle(float Q_angle);
    /**
     * setQbias(float Q_bias)
     * Default value (0.003f) is in Kalman.cpp. 
     * Raise this to follow input more closely,
     * lower this to smooth result of kalman filter.
     */
    void setQbias(float Q_bias);
    void setRmeasure(float R_measure);

    float getQangle();
    float getQbias();
    float getRmeasure();

private:
    /* Kalman filter variables */
    float Q_angle; // Process noise variance for the accelerometer
    float Q_bias; // Process noise variance for the gyro bias
    float R_measure; // Measurement noise variance - this is actually the variance of the measurement noise

    float angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector
    float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
    float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate

    float P[2][2]; // Error covariance matrix - This is a 2x2 matrix
};

#endif
